#include "obj_cart.h"

int obj_cart::cloaded=0;	//class loaded
GLuint obj_cart::cart_textures[CART_NUM];

obj_cart::obj_cart(){
	if (cloaded==0){
		char filename[100];
		for (int i=0;i<CART_NUM;i++){
			sprintf(filename,"./media/objects/obj_cart%d.tga",i);
			cart_textures[i]=load_tga_texture(filename);
			if (cart_textures[i]==(GLuint)-1){
				debug("Can't find right picture");
				return;
			}
		}
		cloaded=1;
	}

	verts.resize(4);
	col_verts.resize(4);
	faces.resize(2);
	UVs.resize(4);
	
	verts[0][0]=-CART_LENGHT/2.;verts[0][1]=-CART_WIDTH/2.;verts[0][2]=0;
	verts[1][0]=+CART_LENGHT/2.;verts[1][1]=-CART_WIDTH/2.;verts[1][2]=0;
	verts[2][0]=+CART_LENGHT/2.;verts[2][1]=+CART_WIDTH/2.;verts[2][2]=0;
	verts[3][0]=-CART_LENGHT/2.;verts[3][1]=+CART_WIDTH/2.;verts[3][2]=0;

	faces[0][0]=0;faces[0][1]=1;faces[0][2]=2;faces[0][3]=0;faces[0][4]=1;faces[0][5]=2;
	faces[1][0]=0;faces[1][1]=2;faces[1][2]=3;faces[1][3]=0;faces[1][4]=2;faces[1][5]=3;

	UVs[0][0]=0;UVs[0][1]=0;
	UVs[1][0]=1;UVs[1][1]=0;
	UVs[2][0]=1;UVs[2][1]=1;
	UVs[3][0]=0;UVs[3][1]=1;
	
	loaded=1;
	transparent=1;

	dx=dy=drot=0;
	weight=0;
	calc_r();
	collide=1;
	cc_e=0.5;
	obj_type=OBJ_TYPE_CART;
}

void obj_cart::draw_cart(){
	draw();
}


void obj_cart::update(float acc,float ddrot){
	while (rot<0){
		rot+=2*M_PI;
		debug("1rot %f\n",rot);
	}
	while (rot>2*M_PI){
		rot-=2*M_PI;
		debug("2rot %f\n",rot);
	}

//	debug("\n\n %f %f %f %f %f %f",x,y,oldx,oldy,x-oldx,y-oldy);

	oldx=x;
	oldy=y;
	oldrot=rot;

	//zratat rychlost v smere a kolmu a podla trenia upravit skutocnu rychlost
	vs = dx*cos(rot)+dy*sin(rot);
	vk = dx*sin(rot)-dy*cos(rot);
	
	if (vk>0)
		vk=minn(0,vk-time_elapsed()*(fe(GSF_PLAYER_FRICTION_SIDE)+vk*vk/fe(GSF_PLAYER_MAXSIDESPEED))/mass);
	else
		vk=maxx(0,vk+time_elapsed()*(fe(GSF_PLAYER_FRICTION_SIDE)+vk*vk/fe(GSF_PLAYER_MAXSIDESPEED))/mass);

	if (vs>0)
		vs=minn(0,vs-time_elapsed()*(fe(GSF_PLAYER_FRICTION_FRONT)+vs*vs/fe(GSF_PLAYER_MAXSPEED))/mass);
	else
		vs=maxx(0,vs+time_elapsed()*(fe(GSF_PLAYER_FRICTION_FRONT)+vs*vs/fe(GSF_PLAYER_MAXSPEED))/mass); 

	dx= vs*cos(rot)+vk*sin(rot);
	dy= vs*sin(rot)-vk*cos(rot);
	
	//stlmim rotaciu

	if (drot>0)
		drot=minn(0,drot-time_elapsed()*(fe(GSF_PLAYER_FRICTION_ROTATE)+drot*drot/fe(GSF_PLAYER_MAXTURN))/mass);
	else
		drot=maxx(0,drot+time_elapsed()*(fe(GSF_PLAYER_FRICTION_ROTATE)+drot*drot/fe(GSF_PLAYER_MAXTURN))/mass); 
	
	//vplyv hraca
	dx+=fe(GSF_PLAYER_SPEED)*acc*cos(rot)*time_elapsed();
	dy+=fe(GSF_PLAYER_SPEED)*acc*sin(rot)*time_elapsed();
	drot+=fe(GSF_PLAYER_TURN)*ddrot*time_elapsed();

	//priratat rychlost k aktualnej polohe
	x+=dx*time_elapsed();
	y+=dy*time_elapsed();
	rot+=drot*time_elapsed();
	

	FOR(i,verts.size()){
		col_verts[i].x=x+verts[i].x*cos(rot)-verts[i].y*sin(rot);
		col_verts[i].y=y+verts[i].y*cos(rot)+verts[i].x*sin(rot);
	}

//	debug("%f %f %f %f %f %f",x,y,oldx,oldy,x-oldx,y-oldy);
}

int obj_cart::check_intersect(const point &a,const point &c,const point &d){
	if ((c.x-a.x)*(d.x-a.x)<=0){
		if (((d.x-c.x)*(a.y-c.y)-(d.y-c.y)*(a.x-c.x))*(d.x-c.x)<=0){
			return 1;
		}
	}
	return 0;
}

int obj_cart::check_intersect(const point &a,const point &b,const point &c,const point &d){
	if (((b.x-a.x)*(c.y-a.y)-(b.y-a.y)*(c.x-a.x))*((b.x-a.x)*(d.y-a.y)-(b.y-a.y)*(d.x-a.x))<=0){
		if (((d.x-c.x)*(a.y-c.y)-(d.y-c.y)*(a.x-c.x))*((d.x-c.x)*(b.y-c.y)-(d.y-c.y)*(b.x-c.x))<=0){
			return 1;
		}
	}
	return 0;
}

float obj_cart::check_plinedst(const point &a, const point &b, const point &c){
//	d = b.y - c.y
//	e = c.x - b.x
//	|dx + ey + f|/sqrt(d^2+e^2)
//	(b.y - c.y)*b.x + (c.x - b.x) * b.y +c = 0
//  c = (c.y - b.y )*b.x + (b.x - c.x ) * b.y

	return abs((b.y - c.y)*a.x+(c.x - b.x)*a.y + (c.y - b.y )*b.x + (b.x - c.x ) * b.y)/sqrt(square(b.y-c.y)+square(c.x - b.x));
}

int obj_cart::check_inside(const point &a,const obj & col){
	int inside=0;
	FOR(i,col.col_verts.size())
		if (check_intersect(a,col.col_verts[i],col.col_verts[(i+1)%col.col_verts.size()])!=0)
			inside++;
		return inside%2;
}



void obj_cart::chcollide (obj & col){
	if (col.collide==0)
		return;
	if ((col.x-x)*(col.x-x)+(col.y-y)*(col.y-y)>(r+col.r)*(r+col.r))
		return;

	collision_my=collision_his=0;
	
	if (col.col_verts.size()>collide_his.size())
		collide_his.resize(col.col_verts.size());

	FOR(i,4)
		if ((collide_my[i]=check_inside(col_verts[i],col))!=0)
			collision_my++;

	FOR(i,col.col_verts.size())
		if ((collide_his[i]=check_inside(col.col_verts[i],*this))!=0)
			collision_his++;
	
//	debug("his %d my %d",collision_his, collision_my);
	
	if (collision_my==0 && collision_his==0)	// ak nic tak chod dalej
		return;

	collide_point=collide_point * 0;

	FOR(i,4)
		if (collide_my[i]!=0)
			collide_point=collide_point+col_verts[i];
	FOR(i,col.col_verts.size())
		if (collide_his[i]!=0)
			collide_point=collide_point+col.col_verts[i];

	collide_point=collide_point/(collision_my+collision_his);
	
	closest_dst=1000000000;
	
	if (collision_his!=0)
		FOR(i,4)
			if (closest_dst > check_plinedst(collide_point,col_verts[i],col_verts[(i+1)%4])){
				closest_dst = check_plinedst(collide_point,col_verts[i],col_verts[(i+1)%4]);
				closest_points[0]=col_verts[i];
				closest_points[1]=col_verts[(i+1)%4];
			}

	if (collision_my!=0)
		FOR(i,col.col_verts.size())
			if (closest_dst > check_plinedst(collide_point,col.col_verts[i],col.col_verts[(i+1)%col.col_verts.size()])){
				closest_dst = check_plinedst(collide_point,col.col_verts[i],col.col_verts[(i+1)%col.col_verts.size()]);
				closest_points[1]=col.col_verts[(i+1)%col.col_verts.size()];
				closest_points[0]=col.col_verts[i];
			}
	
	
	calc_collision(col,collide_point,closest_points[0],closest_points[1]);
	return;
}

void  obj_cart::calc_collision (obj & col, const point & colp,const point & n1,const point & n2){

	debug("collide point: %f %f",collide_point.x,collide_point.y);
	
//	debug("buras chlape");
//	debug("%f %f d:%f %f old:%f %f olddiff:%f %f\n",x,y,dx,dy,oldx,oldy,x-oldx,y-oldy);
	x=oldx;
	y=oldy;
	rot=oldrot;
	
	// zdroj:	 j = −(1 + e)(vap1 · n) / (1⁄ma + (rap × n)2 ⁄ Ia)
	cc_nx=n2.y-n1.y;
	cc_ny=n1.x-n2.x;
	if (cc_nx*(colp.x-x)+cc_ny*(colp.y-y)<0){
		cc_nx=-cc_nx;
		cc_ny=-cc_ny;
	}
	debug("N*Rap = %f", cc_nx*(colp.x-x), cc_ny*(colp.y-y));
	cc_nl=sqrt(cc_nx*cc_nx+cc_ny*cc_ny);
	cc_nx/=cc_nl;
	cc_ny/=cc_nl;
	cc_apx=dx-drot*(colp.y-y)-(col.dx-col.drot*(colp.y-col.y));
	cc_apy=dy+drot*(colp.x-x)-(col.dy+col.drot*(colp.x-col.x));
	debug("cc_ap: %f %f",cc_apx,cc_apy);
	debug("%f %f %f %f %f\n",dx,drot*(colp.y-y),col.dx,col.drot*(colp.y-col.y),colp.y); 

	cc_j=-(1+cc_e)*(cc_apx*cc_nx+cc_apy*cc_ny)/(1+ square((colp.x-x)*cc_ny -cc_nx*(colp.y-y))*3/(CART_WIDTH*CART_WIDTH+CART_WIDTH*CART_WIDTH))-1;
	
	debug("Kreslim debug ciary")
	glBegin(GL_LINES);
		glVertex3f(n1.x,n1.y,0.9);
		glVertex3f(n2.x,n2.y,0.9);
		glVertex3f(n1.x,n1.y,0.9);
		glVertex3f(n1.x+cc_nx,n1.y+cc_ny,0.9);
		glVertex3f(n2.x,n2.y,0.9);
		glVertex3f(n2.x+cc_nx,n2.y+cc_ny,0.9);
	glEnd();
	glPointSize(5.);
	glBegin(GL_POINTS);
		glVertex3f(collide_point.x,collide_point.y,0.9);
	glEnd();
	
	debug("ccj: %f",cc_j);

	debug("cc_j = %f*%f/(1+%f*3/%f %f\n",(1+cc_e),(cc_apx*cc_nx+cc_apy*cc_ny),square((colp.x-x)*cc_ny -cc_nx*(colp.y-y)),(CART_WIDTH*CART_WIDTH+CART_WIDTH*CART_WIDTH),0);

//	debug("cc %f %f %f %f %f\n",colp.x,colp.y,cc_j,cc_nx,cc_ny);

	dx+=cc_j*cc_nx;
	dy+=cc_j*cc_ny;
	drot+=cc_j*((colp.x-x)*cc_ny -cc_nx*(colp.y-y))*3/(CART_WIDTH*CART_WIDTH+CART_WIDTH*CART_WIDTH);
	
	x+=dx*time_elapsed();
	y+=dy*time_elapsed();
	rot+=drot*time_elapsed();

//	debug("%f %f d:%f %f old:%f %f olddiff:%f %f\n",x,y,dx,dy,oldx,oldy,x-oldx,y-oldy);
	
}

void obj_cart::joint_push(obj_cart & prev){
	
	joint_fx=x + (CART_LENGHT+1)*cos(rot)/2;
	joint_fy=y + (CART_LENGHT+1)*sin(rot)/2;
	prev.joint_rx=prev.x - (CART_LENGHT+1)*cos(prev.rot)/2;
	prev.joint_ry=prev.y - (CART_LENGHT+1)*sin(prev.rot)/2;
	joint_x =(mass*joint_fx+prev.mass*prev.joint_rx)/(prev.mass+mass);
	joint_y =(mass*joint_fy+prev.mass*prev.joint_ry)/(prev.mass+mass);
	
	ddx=(joint_x-x)*(sqrt (square(x-joint_x)+square(y-joint_y))-(CART_LENGHT+1)/2)/sqrt (square(x-joint_x)+square(y-joint_y));
	ddy=(joint_y-y)*(sqrt (square(x-joint_x)+square(y-joint_y))-(CART_LENGHT+1)/2)/sqrt (square(x-joint_x)+square(y-joint_y));
	ddrot=atan2(joint_y-y,joint_x-x)-rot;

	if (abs(ddrot)>M_PI)	//atan2 mohol preskocit!!!
		if (ddrot<0)
			ddrot += 2*M_PI;
		else
			ddrot -= 2*M_PI;
	
	dx += ddx/time_elapsed();
	dy += ddy/time_elapsed();
	drot += ddrot/time_elapsed();

	x+=ddx;
	y+=ddy;
	rot+=ddrot;
/*
	ddx=(joint_x-prev.x)*(sqrt (square(prev.x-joint_x)+square(prev.y-joint_y))-(CART_LENGHT+1)/2)/sqrt (square(prev.x-joint_x)+square(prev.y-joint_y));
	ddy=(joint_y-prev.y)*(sqrt (square(prev.x-joint_x)+square(prev.y-joint_y))-(CART_LENGHT+1)/2)/sqrt (square(prev.x-joint_x)+square(prev.y-joint_y));
	ddrot=atan2(prev.y-joint_y,prev.x-joint_x)-prev.rot;

	if (abs(ddrot)>M_PI)	//atan2 mohol preskocit!!!
		if (ddrot<0)
			ddrot += 2*M_PI;
		else
			ddrot -= 2*M_PI;

	prev.dx += ddx/time_elapsed();
	prev.dy += ddy/time_elapsed();
	prev.drot += ddrot/time_elapsed();
	
	prev.x+=ddx;
	prev.y+=ddy;
	prev.rot+=ddrot;
	
/*	dx-=(joint_fx-prev.joint_rx)*JOINT_STRENGHT*time_elapsed();
	dy-=(joint_fy-prev.joint_ry)*JOINT_STRENGHT*time_elapsed();
	prev.dx+=(joint_fx-prev.joint_rx)*JOINT_STRENGHT*time_elapsed();
	prev.dy+=(joint_fy-prev.joint_ry)*JOINT_STRENGHT*time_elapsed();
	*/
}
